#include  "hw_control.h"

#define SPEK_MAX_CHANNEL 7
#define SPEK_FRAME_SIZE 16
static uint8_t spek_chan_shift;
static uint8_t spek_chan_mask;
static bool rcFrameComplete = FALSE;
static bool spekDataIncoming = FALSE;
volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
static void spektrumDataReceive(uint16_t c);

volatile uint16_t Spektrum_enable_cnt=0;
volatile uint8_t Spektrum_enable=0;


volatile uint16_t Stick_value_spektrum[8] = {0,0,0,0,0,0,0,0};

volatile uint8_t spektrum_hires=0;

void spektrumInit(void)
{
  	GPIO_InitTypeDef GPIO_InitStructure;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
	USART_InitTypeDef USART_InitStructure;

	// UART 3 RX
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	USART_InitStructure.USART_BaudRate = 115200;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b ;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx;


	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
	USART_Init(USART2, &USART_InitStructure);
    USART_Cmd(USART2, ENABLE);


}


void USART2_IRQHandler(void)
{
	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
		USART_ClearITPendingBit(USART2, USART_IT_RXNE);
		spektrumDataReceive(USART_ReceiveData(USART2));
	}
}

// UART2 Receive ISR callback
static void spektrumDataReceive(uint16_t c)
{
    uint32_t spekTime;
    static uint32_t spekTimeLast, spekTimeInterval;
    static uint8_t  spekFramePosition;

    spekDataIncoming = TRUE;
    spekTime = micros();
    spekTimeInterval = spekTime - spekTimeLast;
    spekTimeLast = spekTime;
    if (spekTimeInterval > 5000)
        spekFramePosition = 0;
    spekFrame[spekFramePosition] = (uint8_t)c;
    if (spekFramePosition == SPEK_FRAME_SIZE - 1) {
        rcFrameComplete = TRUE;
        Spektrum_enable_cnt = 500;
    } else {
        spekFramePosition++;
    }
}

bool spektrumFrameComplete(void)
{
    return rcFrameComplete;
}

static const uint8_t spekRcChannelMap[SPEK_MAX_CHANNEL] = {1, 2, 3, 0, 4, 5, 6};

void spektrumReadRawRC(void)
{
    static uint32_t spekChannelData[SPEK_MAX_CHANNEL];
    uint8_t b;
	if(spektrum_hires) {
    	// 11bit 2048
    	spek_chan_shift = 3;
    	spek_chan_mask = 0x07;
      } else {
    	// 10bit 1024
    	spek_chan_shift = 2;
    	spek_chan_mask = 0x03;
    }
	
	if(Spektrum_enable_cnt != 0) {
		Spektrum_enable_cnt--;
		if (rcFrameComplete) {
			for (b = 3; b < SPEK_FRAME_SIZE; b += 2) {
				uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
				if (spekChannel < SPEK_MAX_CHANNEL)
					spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
			}
			rcFrameComplete = FALSE;
			Spektrum_enable = 1;

			for(b=0;b<8;b++) {
				if(spektrum_hires) {
					Stick_value_spektrum[spekRcChannelMap[b]] =  988 + (spekChannelData[spekRcChannelMap[b]]>>1);
				} else {
					Stick_value_spektrum[spekRcChannelMap[b]] =  988 + (spekChannelData[spekRcChannelMap[b]]);
				}
			}
		}
	} else {
		Spektrum_enable = 0;
	}
}
